clear
clc
close all
%% 初始化
n = 1000;%迭代次数
fx = @(x) x(1)^2 + 2*x(2)^2 -2*x(1)*x(2);%函数
sx2 = @(x1) (2-x1.^2)./x1;%边界
%dfx = @(x) [2*x(1)-2*x(2);4*x(2)-2*x(1)];%梯度
penalty = @(x,rho) rho*(1/(x(1)^2+x(1)*x(2)-2)-1/x(1)-1/x(2)); %罚函数
fx_pen = @(x,rho) fx(x)+penalty(x,rho);%原函数加上罚函数
syms x1 x2 rho;
x = [x1,x2];
f_sym = fx_pen(x,rho);
dfx_1_1 = diff(diff(f_sym,x1),x1);
dfx_1_2 = diff(diff(f_sym,x1),x2);
dfx_2_2 = diff(diff(f_sym,x2),x2);

dfx = [diff(f_sym,x1);diff(f_sym,x2)];%梯度
H = [dfx_1_1,dfx_1_2;dfx_1_2,dfx_2_2];
%此时的hessian矩阵并不是常数，需要具体运算
% example = subs(H,[[x1,x2],rho],[[1,1.1],1])
% ex_val = double(example);

sample.x = [];
sample.dx = [];
sample.y = [];
x_iteration = repmat(sample,n,1);
%% 绘制计算域
nx = 100;
x_1 = linspace(-sqrt(2),sqrt(2),nx);
bx2 = sx2(x_1);%边界
x_2 = x_1;
for i = 1:1:nx
    for j = 1:1:nx
        y(i,j) = fx([x_1(i),x_2(j)]);
    end
end
pcolor(x_1,x_2,y);
shading interp;
hold on 
plot(x_1,bx2,'r');
hold on
%% 计算
x_iteration(1).x = [1.1;1.1];%起始点在边界之外
% x_iteration(1).x = [0.9;0.9];
x_iteration(1).y = fx(x_iteration(1).x);
%x_iteration(1).dx = dfx(x_iteration(1).x);
iteration = [1];
Y = x_iteration(1).y;
%figure
i = 1;
r = 1;%初始罚因子
x_iteration(1).dx = double(subs(dfx,[[x1,x2],rho],[x_iteration(1).x',r]));
s = -x_iteration(1).dx;%初始的sk
error = norm(x_iteration(1).dx);
while(error(end)>=1e-30 && i<=n)
%for i = 2:1:n
    i = i+1;
    clf
    s0 = s;
    %求Hessian矩阵
    Hess = double(subs(H,[[x1,x2],rho],[x_iteration(i-1).x',r]));
    %梯度下降
    g = x_iteration(i-1).dx;
    alpha = -(g'*s)/(2*s'*Hess*s);
    %alpha = 0.001;
    x_iteration(i).x = x_iteration(i-1).x + alpha*s;

    x_iteration(i).y = fx(x_iteration(i).x);

    %求梯度
    x_iteration(i).dx = double(subs(dfx,[[x1,x2],rho],[[x_iteration(i).x'],r]));
    beta = (norm(x_iteration(i).dx)^2)/(norm(x_iteration(i-1).dx)^2);
    %s = -x_iteration(i).dx+beta*s0;
    s = -x_iteration(i).dx;

    %罚因子衰减
    if mod(i,50)==0
        r = r/10;
    end

    %绘图
    iteration = [iteration,i];
    error = [error,norm(x_iteration(i).dx)];
    Y = [Y,x_iteration(i).y];
    subplot(2,1,1)
    semilogy(iteration,error);
    yticks(10.^(-30:3:0));
    subplot(2,1,2)
    semilogy(iteration,Y);
    yticks(10.^(-60:2:0));
    % hold on
    % x_all = [x_iteration.x];
    % plot(x_all(1,:),x_all(2,:),'Color','g');
    % title(num2str(i));
    pause(0.001);
end